#include "matrix.h"

mat3x3* mat3x3_copy(mat3x3* des, const mat3x3* src)
{
    des->m[0][0] = src->m[0][0];
    des->m[0][1] = src->m[0][1];
    des->m[0][2] = src->m[0][2];
    des->m[1][0] = src->m[1][0];
    des->m[1][1] = src->m[1][1];
    des->m[1][2] = src->m[1][2];
    des->m[2][0] = src->m[2][0];
    des->m[2][1] = src->m[2][1];
    des->m[2][2] = src->m[2][2];
    return des;
}
mat3x3* mat3x3_diag(mat3x3* out, float v)
{
    out->m[0][0] = v;
    out->m[0][1] = 0;
    out->m[0][2] = 0;
    out->m[1][0] = 0;
    out->m[1][1] = v;
    out->m[1][2] = 0;
    out->m[2][0] = 0;
    out->m[2][1] = 0;
    out->m[2][2] = v;
    return out;
}
mat3x3* mat3x3_one(mat3x3* out)
{
    return mat3x3_diag(out, 1.f);
}
mat3x3* mat3x3_zero(mat3x3* out)
{
    return mat3x3_diag(out, 0.f);
}

vec3* mat3x3_mult_v3_copy(vec3* out, const mat3x3* m33, const vec3* v)
{
    out->x = m33->m[0][0] * v->x + m33->m[1][0] * v->y + m33->m[2][0] * v->z;
	out->y = m33->m[0][1] * v->x + m33->m[1][1] * v->y + m33->m[2][1] * v->z;
	out->z = m33->m[0][2] * v->x + m33->m[1][2] * v->y + m33->m[2][2] * v->z;
    return out;
}
vec3* mat3x3_mult_v3(const mat3x3* m33, vec3* v)
{
    vec3 out;
    return vec3_copy(v, mat3x3_mult_v3_copy(&out, m33, v));
}

mat3x3 *mat3x3_mult_mat3x3_copy(mat3x3 *out, const mat3x3 *left, const mat3x3 *right)
{
    #define SrcA00 left->m[0][0]
    #define SrcA01 left->m[0][1]
    #define SrcA02 left->m[0][2]
    #define SrcA10 left->m[1][0]
    #define SrcA11 left->m[1][1]
    #define SrcA12 left->m[1][2]
    #define SrcA20 left->m[2][0]
    #define SrcA21 left->m[2][1]
    #define SrcA22 left->m[2][2]

    #define SrcB00 right->m[0][0]
    #define SrcB01 right->m[0][1]
    #define SrcB02 right->m[0][2]
    #define SrcB10 right->m[1][0]
    #define SrcB11 right->m[1][1]
    #define SrcB12 right->m[1][2]
    #define SrcB20 right->m[2][0]
    #define SrcB21 right->m[2][1]
    #define SrcB22 right->m[2][2]

    out->m[0][0] = SrcA00 * SrcB00 + SrcA10 * SrcB01 + SrcA20 * SrcB02;
    out->m[0][1] = SrcA01 * SrcB00 + SrcA11 * SrcB01 + SrcA21 * SrcB02;
    out->m[0][2] = SrcA02 * SrcB00 + SrcA12 * SrcB01 + SrcA22 * SrcB02;
    out->m[1][0] = SrcA00 * SrcB10 + SrcA10 * SrcB11 + SrcA20 * SrcB12;
    out->m[1][1] = SrcA01 * SrcB10 + SrcA11 * SrcB11 + SrcA21 * SrcB12;
    out->m[1][2] = SrcA02 * SrcB10 + SrcA12 * SrcB11 + SrcA22 * SrcB12;
    out->m[2][0] = SrcA00 * SrcB20 + SrcA10 * SrcB21 + SrcA20 * SrcB22;
    out->m[2][1] = SrcA01 * SrcB20 + SrcA11 * SrcB21 + SrcA21 * SrcB22;
    out->m[2][2] = SrcA02 * SrcB20 + SrcA12 * SrcB21 + SrcA22 * SrcB22;
    return out;
}

mat3x3* mat3x3_mult_mat3x3(mat3x3 *left, const mat3x3 *right)
{
    mat3x3 out;
    return mat3x3_copy(left, mat3x3_mult_mat3x3_copy(&out, left, right));
}
mat3x3 *mat3x3_inverse_copy(mat3x3 *out, const mat3x3 *m33)
{
    float OneOverDeterminant = (1.f) / (
				+ m33->m[0][0] * (m33->m[1][1] * m33->m[2][2] - m33->m[2][1] * m33->m[1][2])
				- m33->m[1][0] * (m33->m[0][1] * m33->m[2][2] - m33->m[2][1] * m33->m[0][2])
				+ m33->m[2][0] * (m33->m[0][1] * m33->m[1][2] - m33->m[1][1] * m33->m[0][2]));

    out->m[0][0] = +(m33->m[1][1] * m33->m[2][2] - m33->m[2][1] * m33->m[1][2]) * OneOverDeterminant;
    out->m[1][0] = -(m33->m[1][0] * m33->m[2][2] - m33->m[2][0] * m33->m[1][2]) * OneOverDeterminant;
    out->m[2][0] = +(m33->m[1][0] * m33->m[2][1] - m33->m[2][0] * m33->m[1][1]) * OneOverDeterminant;
    out->m[0][1] = -(m33->m[0][1] * m33->m[2][2] - m33->m[2][1] * m33->m[0][2]) * OneOverDeterminant;
    out->m[1][1] = +(m33->m[0][0] * m33->m[2][2] - m33->m[2][0] * m33->m[0][2]) * OneOverDeterminant;
    out->m[2][1] = -(m33->m[0][0] * m33->m[2][1] - m33->m[2][0] * m33->m[0][1]) * OneOverDeterminant;
    out->m[0][2] = +(m33->m[0][1] * m33->m[1][2] - m33->m[1][1] * m33->m[0][2]) * OneOverDeterminant;
    out->m[1][2] = -(m33->m[0][0] * m33->m[1][2] - m33->m[1][0] * m33->m[0][2]) * OneOverDeterminant;
    out->m[2][2] = +(m33->m[0][0] * m33->m[1][1] - m33->m[1][0] * m33->m[0][1]) * OneOverDeterminant;
    return out;
}

mat3x3 *mat3x3_inverse(mat3x3 *m33)
{
    mat3x3 out;
    return mat3x3_copy(m33 , mat3x3_inverse_copy(&out, m33));
}

